module normalscr2
implicit none
contains
subroutine gallik(m,theta,likresm)
use linear_operators
use matrixfuns
use datahadler
integer, intent(in):: m
double precision, intent(in):: theta(m)
double precision, intent(out):: likresm
integer, parameter:: a=1000,b=3
integer T,n,i,j
double precision y(a,b)
double precision, parameter:: c9=.999d0
common T,n,y
double precision, parameter:: pi=3.14159265358979d0
double precision lamb(T),delta(n),phi(n),c(n),phi0(n),phi1(n),phi2(n),&
                &alpha1,alpha2,llik,sigmat(n,n),gamma(n),gammat(n,n),&
				&cc(n,n),epst(1:T,1:n),isigmat(n,n),igammat(n,n),alpha0&
				&,fttm,wttm
!call exportvec(theta,"thgaint.txt",5)

delta=theta(1:n)
c(1)=1.0d0
c(2:n)=theta(n+1:2*n-1)
alpha0=theta(2*n)
phi0=vassign(n,theta(2*n+1))
alpha1=c9*(dsin(theta(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(theta(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(theta(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(theta(2*n+5))**2.0d0)
end forall

forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
end forall

forall(i=1:T,j=1:n)
	epst(i,j)=y(i,j)-delta(j)
end forall


gamma=phi0/(1.0d0-phi1-phi2)


gammat=diag(gamma)

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)

igammat=diag(1.0d0/gamma)
sigmat=(cc*lamb(1))+gammat
isigmat=igammat-lamb(1)*(((igammat.x.cc).x.igammat)&
     &/(lamb(1)*dot_product(c,matmul(igammat,c))+1.0d0))


llik=-.5d0*n*dlog(2.0d0*pi)-.5d0*dlog(det(sigmat))&
	    &-.5d0*dot_product(epst(1,:),matmul(isigmat,epst(1,:)))

do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0&
	       &+alpha1*(((fttm)**2.0d0)+wttm)+alpha2*lamb(i-1)


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)

	igammat=diag(1.0d0/gamma)

	sigmat=(cc*lamb(i))+gammat
	isigmat=igammat-lamb(i)*(((igammat.x.cc).x.igammat)&
	     &/(lamb(i)*dot_product(c,matmul(igammat,c))+1.0d0))
	llik=llik-.5d0*n*dlog(2.0d0*pi)-.5d0*dlog(det(sigmat))&
	    &-.5d0*dot_product(epst(i,:),matmul(isigmat,epst(i,:)))
end do
likresm=-llik
end subroutine gallik

subroutine gascr(m,theta,grad)
!lambda=1
use linear_operators
use matrixfuns
use datahadler
integer, intent(in):: m
double precision, intent(in):: theta(m)
double precision, intent(out):: grad(m)
integer, parameter:: a=1000,b=3
integer T,n,i,j,ii
double precision y(a,b)
double precision, parameter:: c9=.999d0
common T,n,y
double precision lamb(T),delta(n),c(n),phi0(n),phi1(n),phi2(n),gamma(n),&
                &sigmat(n,n),gammat(n,n),alpha1,alpha2,&
				&cc(n,n),epst(1:T,1:n),isigmat(n,n),igammat(n,n)
double precision ddelta(m,n),dc(m,n),dgamma(m,n),ent(n,n*n)&
               &,dmut(m,n),dalpha1(m),dalpha2(m),alpha0,dalpha0(m)&
			   &,dlambda(m),ident(n,n),sigeet(n),siget(n),wttm,fttm&
			   &,dfttm(m),dwttm(m),matwtt(m),metn(n),dphi0(m,n),dphi1(m,n),dphi2(m,n)&
			   &,vprb(n),matprb(n,n)
		
!ent=matent(n)

ident=eye(n)
delta=theta(1:n)
c(1)=1.0d0
c(2:n)=theta(n+1:2*n-1)
alpha0=theta(2*n)
phi0=vassign(n,theta(2*n+1))
alpha1=c9*(dsin(theta(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(theta(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(theta(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(theta(2*n+5))**2.0d0)
end forall



forall(i=1:m,j=1:n)
	ddelta(i,j)=0.0d0
	dc(i,j)=0.0d0
	dgamma(i,j)=0.0d0
	dphi0(i,j)=0.0d0
	dphi1(i,j)=0.0d0
	dphi2(i,j)=0.0d0
	dmut(i,j)=0.0d0
end forall
forall(i=1:n)
	ddelta(i,i)=1.0d0
	dphi0(2*n+1,i)=1.0d0
	dphi1(2*n+4,i)=c9*2.0d0*dsin(theta(2*n+4))*dcos(theta(2*n+4))
	dphi2(2*n+4,i)=-(c9*2.0d0*dsin(theta(2*n+4))*dcos(theta(2*n+4)))*(dsin(theta(2*n+5))**2.0d0)
	dphi2(2*n+5,i)=(c9-phi1(i))*2.0d0*dsin(theta(2*n+5))*dcos(theta(2*n+5))
end forall
forall(i=2:n)
	dc(n+i-1,i)=1.0d0
end forall

forall(i=1:m)
	dalpha0(i)=0.0d0
	dalpha1(i)=0.0d0
	dlambda(i)=0.0d0
end forall
dalpha2=dalpha1
dalpha0(2*n)=1.0d0
dalpha1(2*n+2)=c9*2.0d0*dsin(theta(2*n+2))*dcos(theta(2*n+2))
dalpha2(2*n+2)=-dalpha1(2*n+2)*(dsin(theta(2*n+3))**2.0d0)
dalpha2(2*n+3)=(c9-alpha1)*2.0d0*dsin(theta(2*n+3))*dcos(theta(2*n+3))


forall(i=1:T,j=1:n)
	epst(i,j)=y(i,j)-delta(j)
end forall


forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
end forall

forall(i=1:n)
	dmut(i,i)=1.0d0
end forall

gamma=phi0/(1.0d0-phi1-phi2)

gammat=diag(gamma)
igammat=diag(1.0d0/gamma)
forall (i=1:m,j=1:n)
	dgamma(i,j)=((1.0d0-phi1(j)-phi2(j))*dphi0(i,j)-phi0(j)*(-dphi1(i,j)-dphi2(i,j)))&
	           &/(1.0d0-phi1(j)-phi2(j))**2.0d0
end forall
dlambda=((1.0d0-alpha1-alpha2)*dalpha0-alpha0*(-dalpha1-dalpha2))/(1.0d0-alpha1-alpha2)**2.0d0

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)
sigmat=lamb(1)*cc+gammat
isigmat=igammat-(((igammat.x.cc).x.igammat)&
	      &/(dot_product(c,matmul(igammat,c))+(1.0d0/lamb(1))))

forall(i=1:n)
	sigeet(i)=(dot_product(isigmat(i,:),epst(1,:))*&
	          & dot_product(isigmat(:,i),epst(1,:)))-isigmat(i,i)
end forall
siget=matmul(isigmat,epst(1,:))

grad=matmul(dmut,siget)+.5d0*matmul(dgamma,sigeet)&
&+.5d0*dlambda*((dot_product(c,siget)**2.0)-dot_product(c,matmul(isigmat,c)))&
&+.5d0*lamb(1)*(dot_product(c,siget)*2.0d0*matmul(dc,siget)&
&-2.0*matmul(matmul(dc,isigmat),c))



!t>1
do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0+alpha1*((fttm**2.0d0)+wttm)+alpha2*lamb(i-1)

	forall(ii=1:n)
	    metn(ii)=dot_product(igammat(ii,:),c)*dot_product(igammat(:,ii),epst(i-1,:))
	end forall
	!matwtt=matmul(matmul(dgamma,ent),vec2((igammat.x.cc).x.igammat))
	matprb=matmul(matmul(igammat,cc),igammat)
	forall(j=1:n)
		vprb(j)=matprb(j,j)
	end forall
	matwtt=matmul(dgamma,vprb)

	dwttm=-(wttm*wttm)*(-(lamb(i-1)**(-2.0))*dlambda+2.0d0*matmul(dc,matmul(igammat,c))&
	      &-matwtt)
	
	dfttm=dwttm*dot_product(c,matmul(igammat,epst(i-1,:)))&
	     &+wttm*matmul(dc,matmul(igammat,epst(i-1,:)))&
		 &-wttm*matmul(dgamma,metn)-wttm*matmul(dmut,matmul(igammat,c))


	forall (ii=1:m,j=1:n)
		dgamma(ii,j)=dphi0(ii,j)+dphi1(ii,j)*(((epst(i-1,j)-c(j)*fttm)**2.0d0)&
		                                                                   &+(c(j)*c(j))*wttm)&
		           &+dphi2(ii,j)*gamma(j)&
				   &+phi1(j)*2.0d0*(epst(i-1,j)-c(j)*fttm)*&
				   &(-dmut(ii,j)-dc(ii,j)*fttm-c(j)*dfttm(ii))&
				   &+phi1(j)*2.0d0*c(j)*dc(ii,j)*wttm+phi1(j)*c(j)*c(j)*dwttm(ii)+phi2(j)*dgamma(ii,j)
	end forall


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)
	igammat=diag(1.0d0/gamma)


	sigmat=lamb(i)*cc+gammat
	isigmat=igammat-(((igammat.x.cc).x.igammat)&
	      &/(dot_product(c,matmul(igammat,c))+(1.0d0/lamb(i))))
	forall(ii=1:n)
		sigeet(ii)=(dot_product(isigmat(ii,:),epst(i,:))*&
		          &   dot_product(isigmat(:,ii),epst(i,:)))-isigmat(ii,ii)
	end forall
	siget=matmul(isigmat,epst(i,:))

	dlambda=dalpha0+dalpha1*((fttm*fttm)+wttm)+dalpha2*(lamb(i-1))&
	      &+alpha1*(2.0d0*fttm*dfttm+dwttm)+alpha2*dlambda


	grad=grad+matmul(dmut,siget)+.5d0*matmul(dgamma,sigeet)&
		&+.5d0*dlambda*((dot_product(c,siget)**2.0)-dot_product(c,matmul(isigmat,c)))&
		&+.5d0*lamb(i)*(dot_product(c,siget)*2.0d0*matmul(dc,siget)&
		&-2.0*matmul(matmul(dc,isigmat),c))
end do

grad=-grad
end subroutine gascr

subroutine gasimul(m,T,n,theta,y)
use linear_operators
use matrixfuns
integer, intent(in):: m,T,n
double precision, intent(in):: theta(m)
double precision, intent(out):: y(T,n)
integer i,j,irank
double precision, parameter:: c9=.999d0
double precision, parameter:: pi=3.14159265358979d0
double precision lamb(T),delta(n),phi(n),c(n),phi0(n),phi1(n),phi2(n),&
                &alpha1,alpha2,llik,sigmat(n,n),gamma(n),gammat(n,n),&
				&cc(n,n),epst(1:T,1:n),isigmat(n,n),igammat(n,n)&
				&,fttm,wttm,rsigmat(n,n),epstar(n),tol,dmach,alpha0
delta=theta(1:n)
c(1)=1.0d0
c(2:n)=theta(n+1:2*n-1)
alpha0=theta(2*n)
phi0=vassign(n,theta(2*n+1))
alpha1=c9*(dsin(theta(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(theta(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(theta(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(theta(2*n+5))**2.0d0)
end forall


forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
end forall


gamma=phi0/(1.0d0-phi1-phi2)

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)

gammat=diag(gamma)
sigmat=cc*lamb(1)+gammat

tol = 100.0*dmach(4)

call dchfac(n,sigmat,n,tol,irank,rsigmat,n)
rsigmat=.t.rsigmat
call drnnor(n,epstar)
epst(1,:)=matmul(rsigmat,epstar)
y(1,:)=delta+epst(1,:)

do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0+alpha1*(((fttm)**2.0d0)+wttm)+alpha2*lamb(i-1)


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)

	sigmat=(cc*lamb(i))+gammat

	call dchfac(n,sigmat,n,tol,irank,rsigmat,n)
	rsigmat=.t.rsigmat
	call drnnor(n,epstar)
	epst(i,:)=matmul(rsigmat,epstar)
	y(i,:)=delta+epst(i,:)
end do
end subroutine gasimul

!!!
subroutine giveepstar(m,T,n,y,theta,epstar)
use linear_operators
use matrixfuns
use datahadler
integer, intent(in):: m,T,n
double precision, intent(in):: y(T,n),theta(m)
double precision, intent(out):: epstar(T,n)
integer i,j,irank
double precision, parameter:: c9=.999d0
double precision, parameter:: pi=3.14159265358979d0
double precision lamb(T),delta(n),phi(n),c(n),phi0(n),phi1(n),phi2(n),&
                &alpha1,alpha2,llik,sigmat(n,n),gamma(n),gammat(n,n),&
				&cc(n,n),epst(1:T,1:n),isigmat(n,n),igammat(n,n),alpha0&
				&,fttm,wttm,tol,dmach,rsigmat(n,n),irsigmat(n,n)

delta=theta(1:n)
c(1)=1.0d0
c(2:n)=theta(n+1:2*n-1)
alpha0=theta(2*n)
phi0=vassign(n,theta(2*n+1))
alpha1=c9*(dsin(theta(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(theta(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(theta(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(theta(2*n+5))**2.0d0)
end forall

forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
end forall

forall(i=1:T,j=1:n)
	epst(i,j)=y(i,j)-delta(j)
end forall


gamma=phi0/(1.0d0-phi1-phi2)


gammat=diag(gamma)

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)

igammat=diag(1.0d0/gamma)
sigmat=(cc*lamb(1))+gammat
isigmat=igammat-lamb(1)*(((igammat.x.cc).x.igammat)&
     &/(lamb(1)*dot_product(c,matmul(igammat,c))+1.0d0))


tol = 100.0*dmach(4)
call dchfac(n,sigmat,n,tol,irank,rsigmat,n)
irsigmat=.i.(.t.rsigmat)

epstar(1,:)=matmul(irsigmat,epst(1,:))

do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0&
	       &+alpha1*(((fttm)**2.0d0)+wttm)+alpha2*lamb(i-1)


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)

	igammat=diag(1.0d0/gamma)

	sigmat=(cc*lamb(i))+gammat
	isigmat=igammat-lamb(i)*(((igammat.x.cc).x.igammat)&
	     &/(lamb(i)*dot_product(c,matmul(igammat,c))+1.0d0))

	call dchfac(n,sigmat,n,tol,irank,rsigmat,n)
	irsigmat=.i.(.t.rsigmat)

	epstar(i,:)=matmul(irsigmat,epst(i,:))
end do

end subroutine giveepstar

subroutine givey(m,T,n,y,theta,epstar)
use linear_operators
use matrixfuns
use datahadler
integer, intent(in):: m,T,n
double precision, intent(in):: epstar(T,n),theta(m)
double precision, intent(out):: y(T,n)
integer i,j,irank
double precision, parameter:: c9=.999d0
double precision, parameter:: pi=3.14159265358979d0
double precision lamb(T),delta(n),phi(n),c(n),phi0(n),phi1(n),phi2(n),&
                &alpha1,alpha2,llik,sigmat(n,n),gamma(n),gammat(n,n),&
				&cc(n,n),epst(1:T,1:n),isigmat(n,n),igammat(n,n),alpha0&
				&,fttm,wttm,tol,dmach,rsigmat(n,n)

delta=theta(1:n)
c(1)=1.0d0
c(2:n)=theta(n+1:2*n-1)
alpha0=theta(2*n)
phi0=vassign(n,theta(2*n+1))
alpha1=c9*(dsin(theta(2*n+2))**2.0d0)
alpha2=(c9-alpha1)*(dsin(theta(2*n+3))**2.0d0)

forall(i=1:n)
phi1(i)=c9*(dsin(theta(2*n+4))**2.0d0)
end forall
forall(i=1:n)
phi2(i)=(c9-phi1(i))*(dsin(theta(2*n+5))**2.0d0)
end forall

forall(i=1:n,j=1:n)
	cc(i,j)=c(i)*c(j)
end forall


gamma=phi0/(1.0d0-phi1-phi2)


gammat=diag(gamma)

lamb(1)=alpha0/(1.0d0-alpha1-alpha2)

igammat=diag(1.0d0/gamma)
sigmat=(cc*lamb(1))+gammat
isigmat=igammat-lamb(1)*(((igammat.x.cc).x.igammat)&
     &/(lamb(1)*dot_product(c,matmul(igammat,c))+1.0d0))


tol = 100.0*dmach(4)
call dchfac(n,sigmat,n,tol,irank,rsigmat,n)

y(1,:)=delta+matmul(rsigmat,epstar(1,:))
epst(1,:)=y(1,:)-delta

do i=2,T,1
	wttm=lamb(i-1)/(1.0d0+lamb(i-1)*dot_product(c,c/gamma))
	fttm=wttm*dot_product((c/gamma),epst(i-1,:))
	lamb(i)=alpha0&
	       &+alpha1*(((fttm)**2.0d0)+wttm)+alpha2*lamb(i-1)


	gamma=phi0+phi1*(((epst(i-1,:)-c*fttm)**2.0d0)+(c*c)*wttm)+phi2*gamma

	gammat=diag(gamma)

	igammat=diag(1.0d0/gamma)

	sigmat=(cc*lamb(i))+gammat
	isigmat=igammat-lamb(i)*(((igammat.x.cc).x.igammat)&
	     &/(lamb(i)*dot_product(c,matmul(igammat,c))+1.0d0))

	call dchfac(n,sigmat,n,tol,irank,rsigmat,n)
	y(i,:)=delta+matmul(rsigmat,epstar(i,:))
	epst(i,:)=y(i,:)-delta
end do

end subroutine givey
end module normalscr2